# Untitled - By: 86166 - 周二 8月 1 2023

import sensor
import lcd
import image
import time
import random
from modules import ybserial
from modules import ybrgb


# 颜色的LAB值
# L: 明亮度: 0-100 [黑,白]
# a: 红绿色: 正值,红色 负值,绿色 [-128, 127]
# b: 黄蓝色: 正值,黄色 负值,蓝色 [-128, 127]
# 差E = 开跟(差L^2 + 差L^2 + 差L^2)


# lcd, sensor初始化
lcd.init()
sensor.reset()


# sensor配置初始化
# sensor.set_pixformat(sensor.RGB565)
sensor.set_pixformat(sensor.GRAYSCALE)
sensor.set_framesize(sensor.QVGA)
sensor.run(1)
sensor.skip_frames()
sensor.set_auto_exposure(False, 19000)
# 设置摄像头对比度,范围为[-2,+2]
# sensor.set_contrast(2)   # 可以大一点
# 设置摄像头饱和度,范围为[-2,+2]
# sensor.set_saturation(2)   # 可以多给一点
# 设置摄像头亮度，范围为[-2,+2]
# sensor.set_brightness(-3)   # 可以给小一点
# 颜色捕捉时,设置为False
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)


# 颜色识别阈值
# 黑色底板识别相对稳定LAB区间 [10, 30, 10, 100, -20, 60]
# [49, 78, 30, 65, -11, 18] # 测试白色底板,可以,但黑色底板不行
# black_threshold = [0, 30, -128, 0, -128, 0]
# red_on_black = [10, 30, 10, 100, -20, 60]
# red_on_white = [49, 78, 30, 65, -11, 18]
# color_threshold = [0, 30, -128, 0, -128, 0] #黑色的LAB区间,可以增加其他颜色区间,将下方识别的代码循环识别即可


# 红色激光LAB阈值
# red_on_white = (74, 100, 14, 127, -12, -3)
# red_on_white = (86, 94, 15, 101, -16, -7)
red_on_white = (60, 255, -20, 20, -20, 20)
# 存放识别到的红光数据
# laserData = []
# 创建img对象
img = image.Image()
# 创建检测区间
findLen = [140, 140]
# 该框原点坐标
subXY = [(img.width() // 2) - (findLen[0] // 2), (img.height() // 2) - (findLen[1] // 2)]
scope = [subXY[0], subXY[1], findLen[0], findLen[1]]
# 创建串口通信对象
serial = ybserial()
# 设置RGB灯光
RGB = ybrgb()

# 灯光熄灭
def LEDOFF():
    RGB.set(0, 0, 0)

# 控制灯光闪烁
def LEDON():
    global RGB
    color = [random.randint(0, 1), random.randint(0, 1), random.randint(0, 1)]
    RGB.set(color[0], color[1], color[2])

# 绘制识别到的红点
def DrawRecognizeLaser(laserXY):
    global img
    img.draw_rectangle(scope, thicknesss=2)
    if laserXY == [0, 0]:
        return
    laserLen = [10, 10]
    laserRectangle = [laserXY[0] - (laserLen[0] // 2), laserXY[1] - (laserLen[1] // 2), laserLen[0], laserLen[1]]
    img.draw_rectangle(laserRectangle, color=(0, 255, 0), thickness=2)

# 发送坐标数据
def UARTSendLaserXY(laserXY):
    global serial
    if laserXY == [0, 0]:
        return
    # 发送坐标
    sendArray = [0xb3, 0xb3, laserXY[0] - (img.width() // 2), (img.height() - laserXY[1]) - (img.height() // 2), 0x5b]
    serial.send_bytearray(sendArray)
    time.sleep_ms(5)
    print(sendArray[2:4])


while True:
    img = sensor.snapshot()

    recognize = [0, 0]
    recognizeGroup = img.find_blobs([red_on_white], roi=scope, merge=False, pixels_threshold=1, area_threshold=1)

    notFindLen = [10, 10]
    notFindScope = [(img.width() // 2) - (notFindLen[0] // 2), (img.height() // 2) - (notFindLen[1] // 2), (img.width() // 2) + (notFindLen[0] // 2), (img.height() // 2) + (notFindLen[1] // 2)]
    img.draw_rectangle((notFindScope[0], notFindScope[1], notFindLen[0], notFindLen[1]), thickness=2)

    for elem in recognizeGroup:
        if (notFindScope[0] < elem.cx() < notFindScope[2]) and (notFindScope[1] < elem.cy() < notFindScope[3]):
            pass
        else:
            if recognize == [0, 0] or recognize.pixels() < elem.pixels():
                recognize = elem

    if recognize == [0, 0]:
        laserXY = [0, 0]
        LEDON()
    else:
        laserXY = [recognize.cx(), recognize.cy()]
        LEDOFF()

    DrawRecognizeLaser(laserXY)

    UARTSendLaserXY(laserXY)

    lcd.display(img)
